#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;

    //创建订阅者
    tf2_ros::Buffer buffer;//用于订阅者缓存
    tf2_ros::TransformListener sub(buffer);//可以将订阅到的数据存入buffer

    //组织坐标点
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;

    //添加休眠，保证发布方数据发布完毕
    ros::Duration(2).sleep();
    //发布消息
    ros::Rate rate(10);
    while(ros::ok())
    {
        geometry_msgs::PointStamped ps_out;
        ps_out = buffer.transform(ps,"base_link");//tf2转换函数

        ROS_INFO("转换后的坐标是：%.2f %.2f  %.2f ,参考的坐标是：%s",
        
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps_out.header.frame_id.c_str()
        );
        rate.sleep();

        ros::spinOnce();
    }
    return 0;
    
}
